#pragma once

#include"../include/common.hpp"
using namespace cv;
using namespace std;

class TrackRecognition
{
public:
    vector<POINT> LeftEdgePoints;
    vector<POINT> RightEdgePoints;
    vector<POINT> medPoints;
    vector<POINT> SpecialPoints;
    uint16_t Track_H = ROWCOMMON;
    uint16_t Track_W = COLCOMMON;
    uint16_t rowCutUp = 40;  // 顶部切行
    uint16_t rowCutBottom = 40; // 底部切行
    uint8_t seed_track_step = 5;
    POINT start_point_default = POINT(Track_W/2,Track_H-rowCutBottom);  //默认扫点起始点
    POINT start_point = POINT(Track_W/2,Track_H-rowCutBottom);  // 扫点起始点
    float track_error = 0.0f;
    int left_valid_num = 0;
    int right_valid_num = 0;
    float w_rate = 0.0f;
    
    // ========= clear ========= //
    void clear_edge(void)
    {
    
    
      LeftEdgePoints.clear();
      RightEdgePoints.clear();
      medPoints.clear();
    }
    
    
    void trackRecognition_grow(void)
    {
        int w_num = 0;
        for(int y=start_point.y;y>=0;y-=seed_track_step)
        {
          if(this->ImagePath.at<uchar>(y,this->start_point.x)==255)
          {
              w_num++;
          }
        }
        w_rate = seed_track_step*(float)w_num/(start_point.y);
        while(this->ImagePath.at<uchar>(this->start_point.y,this->start_point.x)!=255)
        {
         this->start_point.x += 10;
         if(this->start_point.x>=this->Track_W)
         {
           this->start_point.x = 160;
           break;
         }
        }
        while(this->ImagePath.at<uchar>(this->start_point.y,this->start_point.x)!=255)
        {
         
         this->start_point.x -= 10;
         if(this->start_point.x<=0)
         {
           this->start_point.x = 160;
           break;
         }
        }
        uint16_t start_row = start_point.y;
        uint16_t start_col = start_point.x;
        // 获取起点边缘点   
        LeftEdgePoints.push_back(POINT(0,start_row));   
        RightEdgePoints.push_back(POINT(Track_W-1,start_row));  
        uint8_t pv;
        uint8_t pv_l;
        uint8_t pv_r;
        for(int x = start_col;x>2;--x) // 获取左边起点
        {
            pv = ImagePath.at<uchar>(start_row,x);
            pv_l =  ImagePath.at<uchar>(start_row,x-1);
            if(pv==255 && pv_l==0)
            {
                LeftEdgePoints[0] = POINT(x,start_row);
                break;
            }
        }

        for(int x = start_col;x<Track_W-3;++x) // 获取右边起点
        {
            pv = ImagePath.at<uchar>(start_row,x);
            pv_r =  ImagePath.at<uchar>(start_row,x+1);
            if(pv==255 && pv_r==0)
            {
                RightEdgePoints[0] = POINT(x,start_row);
                break;
            }
        }
        

        POINT TEMP_Lp,TEMP_Rp;
        for(int y = start_row-1;y>rowCutUp;y-=seed_track_step)
        {
            TEMP_Lp = POINT(0,y);
            TEMP_Rp = POINT(Track_W-1,y);
            POINT temp_start_p = POINT((LeftEdgePoints.back().x+RightEdgePoints.back().x)/2,y);
            for(int x = temp_start_p.x;x>2;--x) 
            {
                uint8_t pv = ImagePath.at<uchar>(y,x);
                uint8_t pv_l =  ImagePath.at<uchar>(y,x-1);
                if(pv==255 && pv_l==0)
                {
                    TEMP_Lp = POINT(x,y);
                    break;
                }
            }

            for(int x = temp_start_p.x;x<Track_W-3;++x) 
            {
                uint8_t pv = ImagePath.at<uchar>(y,x);
                uint8_t pv_r =  ImagePath.at<uchar>(y,x+1);
                if(pv==255 && pv_r==0)
                {
                    TEMP_Rp = POINT(x,y);
                    break;
                }
            }

            LeftEdgePoints.push_back(TEMP_Lp);
            RightEdgePoints.push_back(TEMP_Rp);
      }
    }
    
   
   // main=================
    void trackRecognition(Mat &imageBinary)
    {
        ImagePath = imageBinary;
        clear_edge();
        trackRecognition_grow();
        get_valid_point();
        cal_error();
        
    }


    // get_valid_point ============================
    void get_valid_point(void)
    {
        left_valid_num = 0;
        right_valid_num = 0;
        POINT last_point = *LeftEdgePoints.begin();
        
        for (auto it_l = LeftEdgePoints.begin(); it_l < LeftEdgePoints.end(); ++it_l)
        {
          
            if(cal_distance(*it_l,last_point)>=20)
            {
                break;
            }
            left_valid_num++;
            last_point = *it_l;
        }
        
        last_point = *RightEdgePoints.begin();
        for (auto it_r = RightEdgePoints.begin(); it_r < RightEdgePoints.end(); ++it_r)
        {
            if(cal_distance(*it_r,last_point)>=20)
            {
                break;
            }
            right_valid_num++;
            last_point = *it_r;
        }
        
    }
    
    
    // draw_edge =================================
    void draw_edge(Mat &draw_image)
    {
        circle(draw_image,Point(start_point.x,start_point.y),5,Scalar(100,199,255),-1);
        //circle(draw_image,Point(129,18),5,Scalar(0,199,255),-1);
        //circle(draw_image,Point(114,18),5,Scalar(0,199,255),-1);
        //circle(draw_image,Point(114,49),5,Scalar(0,199,255),-1);
        //circle(draw_image,Point(RightEdgePoints[0].x-145,RightEdgePoints[0].y),5,Scalar(0,255,0),-1);
        //circle(draw_image,Point(RightEdgePoints.back().x-45,RightEdgePoints.back().y),5,Scalar(0,255,0),-1);
        
        
        for (auto it = LeftEdgePoints.begin(); it < LeftEdgePoints.end(); it++)
        {
            if(it<LeftEdgePoints.begin()+left_valid_num)
            {
                circle(draw_image,Point((*it).x,(*it).y),3,Scalar(0,0,255),-1);
            }
            else
            {
                circle(draw_image,Point((*it).x,(*it).y),3,Scalar(0,200,255),-1);
            }
            
        }
        
        for (auto it = RightEdgePoints.begin(); it < RightEdgePoints.end(); it++)
        {
            if(it<RightEdgePoints.begin()+right_valid_num)
            {
                circle(draw_image,Point((*it).x,(*it).y),3,Scalar(255,0,0),-1);
            }
            else
            {
                circle(draw_image,Point((*it).x,(*it).y),3,Scalar(255,200,0),-1);
            }
            
        }

        for (auto it = medPoints.begin(); it < medPoints.end(); it++)
        {
            int med_valid_num = get_min(left_valid_num,right_valid_num);
            if(it<medPoints.begin()+med_valid_num)
            {
                circle(draw_image,Point((*it).x,(*it).y),3,Scalar(255,0,255),-1);
            }
            else
            {
                circle(draw_image,Point((*it).x,(*it).y),3,Scalar(255,255,255),-1);
            }
        }
        putText(draw_image,to_string((int)track_error),Point(start_point.x,start_point.y),2,1.0,Scalar(0,0,255));
    }
    
    
    // cal_error ====================================
    void cal_error()
    {    
        medPoints.clear();
        for(uint16_t i=0;i<LeftEdgePoints.size();++i)
        {
          medPoints.push_back(POINT((LeftEdgePoints[i].x+RightEdgePoints[i].x)/2,(LeftEdgePoints[i].y+RightEdgePoints[i].y)/2));   
        
        }
        int med_valid_num = get_min(left_valid_num,right_valid_num);
        //double s = sigma(medPoints,med_valid_num);
        //print(s);
        int sum_dis = 0;
        float t = 0;
        for (auto it = medPoints.begin(); it < medPoints.end(); it++)
        {
          if(it<medPoints.begin()+med_valid_num)
          {
              t = (*it).x - Track_W/2;
              if(my_abs(t)<=6)
              {
                t *= 0.199f;
              }
              else if(my_abs(t)> 6 && my_abs(t) <= 13)
              {
                  t *= 0.529f;//0.23 0.229
              }
                else if(my_abs(t)> 13&& my_abs(t) <=24)
              {
                t*= 0.99;//0.612
              }
              else if(my_abs(t)> 24&& my_abs(t) <=32)
              {
                t*= 1.39f;//0.684
              }
                else if(my_abs(t)> 32 && my_abs(t) <=55)
              {
                t*= 1.58f;//0.757
              }
              else if(my_abs(t)>55 && my_abs(t) <=80)  
              {
                t *=1.88f;
              }
              else
              {
                t *= 2.5f;
              }
              sum_dis += t*turn_function(((float)(*it).y/Track_H));
              //sum_dis += t;
          }
        }
        track_error = sum_dis/(med_valid_num+1);
    }
public:
    Mat ImagePath;
    Mat struct_ero = getStructuringElement(0,Size(7,7));
    Mat struct_ero_farm = getStructuringElement(0,Size(50,50));
    
};

